
0 


2 


o 


7 


KINEMATICS & CONTROL ALGORITHM 
DEVELOPMENT AND SIMULATION FOR A REDUNDANT 
TWO- ARM ROBOTIC MANIPULATOR SYSTEM 


Michael P. Hennessey, Paul C. Huang, and Charles T. Bunnell 
Advanced Systems Center 
FMC Corporation 
1200 South Second Street 
Minneapolis, MN 55459-0043 


Abstract 

An efficient approach to cartesian motion and force control of a 7 degree of freedom (dof ) manipulator is 
presented. It is based on extending the active stiffness controller to the 7 dof case in general and use of an 
efficient version of the gradient projection technique for solving the inverse kinematics problem. Cooperative 
control is achieved through appropriate configuration of individual manipulator controllers. In addition, 
other aspects of trajectory generation using standard techniques are integrated into the controller. The 
method is then applied to a specific manipulator of interest (Robotics Research T-710). Simulation of the 
kinematics, dynamics, and control are provided in the context of several scenarios; one pertaining to a 
noncontact pick and place operation, one relating to contour following where contact is made between the 
manipulator and the environment, and one pertaining to cooperative control. 

1 Introduction 

Cartesian motion and force control of a manipulator is needed for many different types of automation applications such as 
material handling and assembly [1]. Because of the complexity of some potential applications (e.g. in space and in certain 
military environments) and because of the inherent limitations of many 6 dof manipulators (e.g. singularity problems), 
7 (or more) dof manipulators are being proposed for these applications. As a result, there are many interesting and 
challenging problems, particularly with respect to kinematics, control algorithms, and controller implementation aspects. 
Kinematic problems stem largely from two sources: (1) the inverse positional kinematics solution is not unique, and (2) 
it typically does not exist in closed form. As a consequence of the nonclosed form issue, control is complicated from the 
standpoint that highly modular approaches may not be viable (e.g. use of individual joint position servos). Also, despite 
the continual increase in performance and decrease in cost of controller hardware, algorithm efficiency is still an issue. 
Below, the focus is on efficient motion and force control of a 7 dof articulated manipulator. 

For the 7 dof manipulator case, few kinematic configurations permit closed form inverse positional (in general ’’loca- 
tion”) kinematic solutions. For the remaining cases, other approaches must be taken, such as use numerical iteration to 
solve for the inverse positional kinematic solution or knowing the inverse Jacobian, servo at the cartesian level. Baker 
and Wampler [2] refer to all kinematic inversion methods as either ’’global” or ’’local” methods. In both cases, the redun- 
dancy can be used to optimize a criterion of interest. With respect to the first approach, convergence and computational 
efficiency can be a serious issue and perhaps somewhat suprisingly, it may not always be necessary to calculate the 
joint angles corresponding to a particular end effector location, hence obviating the need to solve the inverse positional 
kinematics problem. The second approach avoids the difficulties associated with inverting the positional kinematics, but 
requires a different controller implementation. A recent technique in this catagory is the gradient projection technique 

[2.3]- 

Many different control algorithms have been proposed for motion and force control of a manipulator, including: (1) 
hybrid control, (2) modified hybrid control, (3) active stiffness control, (4) modified resolved acceleration control, (5) 
impedance control, (6) operational space control, (7) free joint control, and (8) modified free joint control [4]. At first 
glance it appears as though there are many radically different control schemes, when in fact there are not [5]. All of the 
above control laws fit into roughly only two catagories: (1) ’’hybrid” control and (2) active stiffness control. However, 
at a practical level both types of controllers can perform hybrid type control depending on how the control system is 
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configured. In hybrid control, position and force are controlled in basically orthogonal directions, and in stiffness control 
the nominal position is controlled and its endpoint stiffness specified. 

Based on the above discussion there are many different approaches for solving the 7 dof manipulator kinematics 
and control problem. In the interests of stability coupled with a desire to minimize computational requirements (for 
implementation reasons primarily), the basic approach taken in this paper is to combine the active stiffness controller 
with an efficient version of the gradient projection technique. Also, since its origination, the active stiffness controller 
(or a slight variation of it) has been experimentally verified to work on several nonredundant manipulators and is well 
known for being computationally efficient (e.g. in [6]). By appropriately configuring individual manipulator controllers, 
cooperative control can be achieved. 


2 General Formulation 

The general block diagram of the system showing the active stiffness controller extended to 7 dof is depicted in Figure 1. 
The controller consists of the following major elements: (1) location feedback loop, (2) joint rate feedback loop, (3) force 
feedback loop, (4) torque compensation, (5) calculation of location error, (6) direct kinematics, (7) gravity compensation, 
and (8) trajectory generator. The location (position and orientation) feedback loop determines the manipulator’s effective 
stiffness and differs from the original formulation in that the servoing is done at the cartesian level because of the nonclosed 
form inverse positional kinematics issue. To determine the cartesian feedback contribution we observe that the location 
feedback given by [6] is: 

K e S9 = (J t KJ)8§_= (J t K){J60) = (J T K)6X (1) 

so that J T K is the cartesian location feedback. Here J is the manipulator Jacobian of the end effector location expressed 
in world coordinates, K is the diagonal arm stiffness matrix, 80 is the joint error vector, and 8X is the resulting cartesian 
error vector expressed in world coordinates. We note that the feedback from Equation (l) ignores the nullspace of 
the Jacobian. The joint rate feedback loop (needed for stability purposes), the force feedback loop, and the torque 
compensation loop characterized by G which is especially useful for contact situations, follow essentially as before [6]. 
Calculation of the location error is defined by A, the 4x4 homogeneous error transformation given by: 

1 — 6z 6y Ax 

8z 1 -8x A y 

—8y 8x 1 A z 

0 0 0 1 . 

where T c and T a are the commanded and actual manipulator transformations and SX_ T — [Ax, Ay, Az,8x } 8y, 6z\. We 
note that A must be in world coordinates and that average values for 8x,8y, and 8z may be derived as implied by 
Equation (2). Finally, the direct kinematics, gravity compensation, and the trajectory generator will be developed in 
conjunction with application to a particular manipulator. 



3 Application to a Particular 7 DOF Manipulator 

The general formulation will now be applied to the Robotics Research T-710 manipulator [7]. 


3.1 Direct Kinematics 


The location of the 7th joint’s coordinate frame in base coordinates ( Tj a<e ) may be expressed as the product of successive 
4x4 homogeneous transformation matrices (A|_ x ) and is given by [8,9]: 


Tbate — A\ ate A*A2A\A\AtA e 


( 3 ) 


where: 


with Pi and r i given recursively by: 
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( 5 ) 


and: 

U = U-l + P i-^i-l,i ^ 

Here ci = cos 0i,3i = sin#,, h i _ 1 i is the vector from the origin of the i — 1th coordinate frame to the ith coordinate 
frame expressed in the i — 1th coordinate frame, and denotes the position vector of joint i in world coordinates with 
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representative kinematic data (i.e. given in the Appendix. For the Robotics Research T-710 manipulator 

shown schematically in Figure 2 the total manipulator transformation T^ ase may be shown to be given by (excluding the 
base and end effector transformations): 

3x &X Px 

Sy dy Py 

S z dz Pz 

0 0 1 

where the positional entries are given by: 

Px = — (ciS3 + Si C 2 C 3 )(/l34x 4~ h,4BxS4 — ft-45 z C4 — ft-66j/ 5 4) 

4- 3l 32 (/l34y — 7l45xC4 — fc.45xS4 4* h{,6yC4) 

— h 23x3132 + h,23zSlC2 ~ hl2xSl 

py ~ ( — S 1 S 3 + ClC2C3)(/l34x + h,4B x S4 ~ h,45 z C4 — fo&6yS4) 

ClS2(^34v ^ 45 xC 4 ^46*^4 4~ ^663/^4) (®) 

+ h, 23 xCl 32 — h 23 zC\C 2 + hl 2 xC\ 

Pz = •® 2 ^' 3 (^' 34 x "f~ ^ 45 x ^4 ^■ 45 i *'4 ^ 563 /^ 4 ) 

4" £2(^343/ ^45x^4 ^45z^4 4“ ^663/^4) 

— h-23xC2 — h,23zS2 4" hl2y 

We note that Equation (8) represents three equations in four unknowns or the mechanical decoupling of the major and 
minor portion of the manipulator linkage. While not proven here, because of certain offsets (i.e. h i _ 1 { ) these equations 
can not be solved in closed form. With Tj a<< defined from above we can include the base and end effector transformations. 
For now, let: 
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so that the entire transformation (T) is given by: 
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( 11 ) 


3.2 Gravity Compensation 

Gravity compensation is used to null out the effects of gravity loading of the manipulator, thereby lessening the role of 
feedback and improving performance. Of course for space applications this is not needed. Compensation is achieved by 
calculating anticipated torques driven by gravity only and uses the recursive Newton-Euler formulation. These torques 

{—gravity) are 6 iven b y [ 9 3 : 

Tgravit yi = t,- (12) 


where: 

U = U - H + (Hi+1 - u) X i +1 + hi,i X mig (13) 

/. — l i+ i + m »3 ( 1 4 ) 

hit = (15) 

Here is the ith joint axis unit vector in world coordinates, r C3 , is the center of gravity vector of the ith link in the 
ith coordinate frame, f. and i ^ are the applied forces and torques to joint t, ro; is the mass of link i, and 5 is the gravity 
vector expressed in world coordinates ([0, — 9.81m/sec 2 , 0] T ). 
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3.3 Trajectory Generator 

The purpose of the trajectory generator is to provide the controller with smooth input trajectories. In general this 
applies to both cartesian (in world or end effector coordinates) and joint interpolated motion for both autonomous and 
teleoperated cases. The focus here is on the most difficult and interesting case; namely autonomous cartesian control (in 
world coordinates). Figure 1 illustrates the specific inputs and outputs of the trajectory generator. Inputs are the initial 
joint angles (0^) (at point ”A”), the commanded cartesian location transformation ( Tb ) (point ”B” expressed as three 
positions and three Euler angles), the travel time (T), and a normalized acceleration parameter (a > 4/T) - assume a 
symmetric trapezoidal velocity profile. Time varying outputs are the location transformation (T(t)) and the joint rates 
(0(t)). The location transformation may be derived without knowledge of the inverse kinematics and 0(<) will be derived 
using the gradient projection technique. Also, when properly configured the trajectory generator can be used to solve 
the inverse positional kinematics problem by providing an initial guess for the joint angles and saving the last set of 
angles available internally to the trajectory generator. Of course the solution is not unique. 


3.3.1 Location Transformation 

To permit smooth simultaneous changes in orientation and position of the end effector, the required orientation change 
takes place about a constant vector k in world coordinates while the origin of the end effector’s local coordinate frame 
is translating along a straight line. In both cases, trapezoidal velocity profiles are used to smooth the motion. This is 
equivalent to use of the following transformation to describe the end effector’s time varying location: 


T(i) = 


Rot(k,e k (t)) p(t) 
o 1 


(16) 


where 9 k ,p = [&,y,z] T have trapazoidal velocity profiles (for 0 < t <T) with symmetric ramp slopes ±ot and Rot(k,9 k ) 
is a rotation matrix corresponding to a rotation of 9 k about a constant vector k in world coordinates given by [8]: 


Rot(k, 9 k ) 


k x k x vers9 k +cos0j. 
k x k y vers9 k -f- k z sin0fe 
k x k z vers9 k — k y sin9 k 


k y k x vers9 k — k z sin 9 k 
kyk y vers9k + cos0*. 
k y k z vers9 k -f k x sin0fe 


k z k x vers9 k -f k y sin0fc 
k z k y vers9 k — k x sh\9 k 
k z k z vers9k + cos 9 k 


(17) 


Here vers9 k = 1 — cos0fc. The vector k = [fcj,, k y , k z ] T and 0j.(T) are evaluated in detail in Paul [8] and use the following 
net rotation matrix as input: 

A orientation — TB3x3T A 3 xi (18) 

with Ta 3 x 3 ,Tb 3 x 3 referring to their respective orientation submatrices. 


3.3.2 Efficient Gradient Projection Technique 

Overview In addition to using the manipulator’s joints to permit cartesian end effector motion the manipulator 
because of its redundancy possesses self-motion; that is, the joints can move without the end effector moving. The 
gradient projection technique uses this motion of the manipulator to attempt to optimize a criterion of interest and has 
been proposed recently by several researchers [2,3]. The self-motion of the arm is characterized by the nullspace of the 
Jacobian (i.e. 9\J9 = 0) and it is desired to express the self-motion joint rates as the gradient of a potential function H 
of interest projected onto the nullspace so that: 


Lu„ = a-J'J)wn (is) 

where J* is the Moore-Penrose psuedo-inverse of J (i.e. J T [JJ T ] — x ) and k is the nullspace gain. In the interests of 
implementation, an optimized version of the gradient projection technique is applied to the manipulator (i.e. J * is 
never actually computed). It was developed and is described in detail in Dubey et al [3]. Basically, the technique 
takes advantage of the existence of a 3x3 0 block in the Jacobian for spherical wristed manipulators and an assumption 
regarding singularities induced by the remaining 4 joints. Specifically, which of the first four columns of the Jacobian 
when removed still permits the remaining matrix to be invertible. The end result consists of several simplified sets of 3 
linear equations in 3 unknowns to be solved, which can be done quite easily. 1 

1 A practical detail concerns the use of a numerical integrator to estimate 0(t) for use in evaluating the inverse Jacobian. As an 
aside, this estimate is subject to drift and is not accurate enough to serve as a joint position command signal, but good enough for 
evaluating J . With respect to integration techniques, both a fourth order Runge-Kutta and a simple Euler integrator have been 
used with success for reasonable speeds (for actual implementation the Euler integrator is perferred because of its simplicity). 
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Application to Robotics Research T-710 Manipulator Major inputs to the efficient gradient projection 
technique are the manipulator Jacobian which for efficiency reasons only is expressed in the third coordinate frame for 
the wrist (not the end effector) and the desired potential function. We will also have reason to evaluate the world to 
third coordinate frame transformation, the end effector to wrist velocity transformation and for convenience J will be 
evaluated in this section (recall that it is needed in the location and force feedback loop). 

The ith ( i = 1, 2, ...,7) column of the wrist Jacobian expressed in the third coordinate frame is given by: 


[ ■>? L = 


— 3t X (-3w -3i) 


( 20 ) 


where s 3i ,r 3i , and r 3w are the ith joint axis unit vector, the ith joint location vector, and the wrist location vector 
expressed in the third coordinate frame, respectively. For the Robotics Research T-710 manipulator the Jacobian J ™ is 
given by: 


J? = 
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( 21 ) 


where the lengthy entries are given in [10]. 

Next, an example use of the potential function will be given. In general one may envision it to be some combination 
(likely a weighted sum) of various subpotential functions associated with singularity avoidance, joint limit avoidance, or 
other criteria of interest of a heuristic nature. To attempt to avoid the wrist singularity, one could choose a potential 
function like H = 3 % to be maximized. For the joint limit problem, one may attempt to minimize H — 5X =1 (0«— ^center) - 
Finally, as a heuristic example, if it is desired to keep the elbow ”up” perhaps for collision avoidance reasons, one would 
like the y world component of the —y$ axis (i.e. C 2 ) to be large (see Figure 2), so choose H = (C 2 4- l) 2 to be maximized 
with k = 1. Figure 3 shows the effect of this particular potential function as the manipulator’s end effector traverses 
along a straight line. 

The transformation projecting the end effector velocities in the world coordinate frame (A) onto the third coordinate 
frame is given by: 
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f} 3 

world 
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l world 
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( 22 ) 


where: 


(23) 


— C 1 S 3 — S 1 C 2 C 3 —S 2 C 3 
J^world — 1 ^2 C 2 

. C1C3 — 3 j C 2 S3 —S 2 S 3 

The end effector to wrist velocity transformation allows one to transform commanded end effector velocities into 
corresponding wrist velocities and is given by: 


— Sl$3 + C 1 C 2 C 3 

— cis 2 
S 1 C 3 + C 1 C 2 S 3 J 


V w — 
~ 3 


Tree e« 

V : 3 -«3 


x r. 


(24) 

(25) 


where r\ e is given by: 


hl e ex[— C43{,37 ~ C7(s43 6 — C 4 C 6 Cg )] — ft.7«y(34C6 + C 4 C 5 S 6 ) 
h7eex [— 343537 + C7(C436 + 34 C 5 C 6 )] + /l7eey(c4C6 — SiC^Se) 
— h7eex(ci,S7 -f S 5 C 6 C 7 ) + fc-7eey35S6 


(26) 


and V and co™ are the translational and rotational velocities of the wrist expressed in the third coordinate frame. 

Having already evaluated the Jacobian needed for trajectory generation (J ™ ), by using the above relationships we 
can evaluate the Jacobian needed for location and force feedback (J) which describes the end effector cartesian rates in 
the world coordinate frame by means of: 


where: 


J = 


(Ko r l d ) T (J?)v +(Koru) T 


(j?u x vru x 

(Rlorldfm* 


(j?) U 7 x ; 


(Jr)v( 3x7) 


(7Dv(3>!7) 
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”1 

. (•£%(». 7) _ 


[ (•73 U )u.l{3xl) ( Jr )w2( 3scl ) 

(jr )u<7(3a;i) 

] 


(27) 


(28) 
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4 Kinematics, Dynamics, and Control (KDAC) Simulation 

To determine anticipated performance of the above described controller and gain confidence in the approach, various sim- 
ulation tools were developed/aquired and the kinematics, dynamics, and control were simulated for the Robotics Research 
T-710 manipulator. The simulation tools of KDAC consist primarily of MATRIXx/AR (Automation and Robotics) [9] 
augmented with application ’’blocks” for performing kinematic and control functions and a graphics animation package 
for displaying the results. Rigid body manipulator dynamics are modeled using appropriately configured blocks arranged 
to support the recursive Newton-Euler formulation of dynamics. Ideal actuators are assumed as an initial baseline, al- 
though this need not be a constraint in the simulation (e.g. actuator inertia and frictional effects can be easily included). 
Representative manipulator data used in the simulation are given in the Appendix. Several scenarios were simulated 
based on Jackson [11] which are representative of tasks required of general purpose 7 dof articulated manipulators. The 
first scenario is concerned with noncontact cartesian position control, the second pertains to contour following of a flat, 
compliant surface, and the third pertains to cooperative control. While a formal stability analysis was performed only for 
the first scenario, for the other scenarios, the effects of such parameters as the controller frequency, the stiffness matrix, 
and the environmental stiffness were investigated empirically. 

4.1 Cartesian Position Control 

Figure 4 illustrates the performance of the system (position command/actual plots) as the end effector attempts to move 
along a straight line approximately in the x WO r id direction while retaining its orientation with an average speed of 6 
inclies/sec. The magnitude of the worst tracking error is 4 mm and after an additional 0.1 second the error drops to 
less than 1 mm (for all directions). When the controller frequency is changed from 100 Hz to 25 Hz the behavior of 
the system is virtually identical (not shown here). Also, when the stiffness matrix is decreased uniformly by a factor 
of 100, one notices a fairly insignificant degradation in performance as shown in Figure 5. In conclusion, for a modest 
speed of 6 inches/sec, the system is fairly robust to changes in the controller frequency and the stiffness matrix when 
performing noncontact straight line motion. A local stability analysis was performed using a continous linear model at 
a particular location. The resultant eigenvalues of the closed loop system were all stable (i.e. in left half plane) and 
when the rate gain was set to zero, all of the eigenvalues were on the imaginary axis. As expected then the rate gain 
introduces damping and pulls the open loop poles off of the ju axis into the left half plane. 


4.2 Contour Following 

Contour following is a generic process in which the manipulator’s end effector maintains contact with the surface of the 
environment while traversing along it. For the specific simulations performed, the manipulator attempted to maintain 
contact with a flat compliant surface parallel to the world xz plane while moving laterally along the surface approximately 
in the x-worid direction. Modeling of the interaction between the manipulator and the environment assumed a massless 
spring; in general an upgrade of this would be to include mass and damping effects (i.e. ’’impedance”). Figure 6 shows 
the nominal performance of the system for an environmental stiffness ( K env ) of 10 kN/m. It is that of a stable lightly 
damped system (some oscillation). As the controller frequency decreases to 25 Hz the performance degrades considerably 
(see Figure 7) and is unstable. For a very stiff environment (i.e. K en v > 1 MN/m) the system is asymptotically stable 
(Figure 8), but with considerable oscillation. Both the controller frequency and the environmental stiffness are seen to 
significantly affect the performance of the system. 

4.3 Cooperative Control 

Modeling cooperative was treated as an extension of the manner in which contour following was modeled. To simplify 
matters somewhat (i.e. ignore certain coupling effects), the interaction between the two manipulators was modeled as 
a massless spring and only translational motion was considered. Figure 9 shows the performance of the system as both 
manipulators laterally translate 0.94 m in the ~x wor u direction while attempting to maintain a 10 N compression force 
on the spring (in the x^orid axis direction) that ties the two arms together. Wihin numerical limits, convergence to the 
steady-state force level is achieved after 0.6 sec of the 3 second trajectory. 

5 Implementation Issues 

Of primary importance in the interests of actual implementation are the details of the bridge between the simulation 
environment and actual implementation and the computational burden of the above algorithms. This is especially true 
since for convenience purposes a hybrid approach was taken in developing the necessary simulation blocks (i.e. some are 
standard blocks and some are custom blocks). Of course, there are many other implementation issues, such as hardware 
considerations, etc. which will not be discussed here. With respect to the first issue, for implementation purposes only 
the gravity compensation and the direct kinematics block would follow the algorithm based on the standard blocks 
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Figure 4 Position (x^j-w) command/ actual! versus time - scenario 

number 1 (100 H», nominal K) 



Figure 6 Poaitio* (y^crtd) act util versus time — scenario number 2. 
(100 Hz, K< nv - 10 kN/m) 



Figure 5 Position (x wc>r ^) command/actual versus time — scenario ‘ 
number 1 (100 Ha, nominal K/100) 



Figure 7 Position (y*. 0 rM) actual versus time — scenario number 2 
(25Hz, K env = lOkN/m) 


(mentioned earlier), while all other blocks would be high speed custom blocks suitable for implementation. For the 
second issue, timing studies based on actual timing of the trajectory generator code (not provided here) indicate that 1 
MC68020 HP could comfortably implement the entire algorithm at rates exceeding 100 Hz. 


6 Conclusions and Future Work 

The active stiffness controller when combined with the gradient projection technique for control of a particular 7 dof 
manipulator (Robotics Research T-710) appears to work satisfactorily based on preliminary simulation studies for noncon- 
tact situations and modest controller frequencies (e.g. > 25 Hz). For the more difficult case where contact is established 
between the manipulator and the environment or for simple cooperative control, the system is stable when an individual 
manipulator’s environment is somewhat compliant ( K env < 1 kN/m) and the controller frequency is sufficiently high 
(i.e. > 100 Hz). In addition, the computational intensity of the algorithm is quite low and timing studies suggest that 
1 MC68020 up could implement the algorithm at rates exceeding 100 Hz. Future work could entail: (1) using the 
torque compensation loop to achieve more stable results when performing contour following of fairly rigid surfaces (i.e. 
K e nv > lfcA^/m), (2) a more extensive local stability analysis, (3) include a more realistic actuator and contact dynamic 
model, and (4) verification on actual hardware. At the time of this writing some of this work is ongoing. 









Figure 9 Relative position error versus time - scenario number 3 (100 i/ 2 , K tpr ing = 7,500 N/m) 
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A Robotics Research T-710 Manipulator Kinematic Data (Repre- 
sentative) 
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B Robotics Research T-T10 Manipulator Dynamic Data (Repre- 
sentative) 


link 1 mass mi = 10 kg 

link 1 center of gravity location r cgi — [0,0, 0] T (m) 
link 1 inertia tensor A, i = diag[0. 1,0. 1,0.1] (kgm 2 ) 
link 2 mass m2 = 10 kg 

link 2 center of gravity location r cgg = [0,0, 0] r (m) 
link 2 inertia tensor A, 2 = diag[0. 1,0. 1,0.1] (kgm 2 ) 
link 3 mass m3 = 10 kg 

link 3 center of gravity location r cgj = [ 0 , 0 , 0 ] T (m) 
link 3 inertia tensor 7 j, 3 = diag[ 0 . 1 , 0 . 1 , 0 . 1 ] (kgm 2 ) 
link 4 mass m 4 = 4 kg 

link 4 center of gravity location r cgt = [ 0 , 0 , 0 ] T (m) 
link 4 inertia tensor A,* = diag[ 0 . 1 , 0 . 1 , 0 . 1 ] (kgm 2 ) 


link 5 mass m B = 4 kg 

link 5 center of gravity location r = [ 0 , 0 , 0 ] T (m) 
link 5 inertia tensor A, 5 = diag[ 0 . 1 , 0 . 1 , 0 . 1 ] (kgm 2 ) 
link 6 mass m» = 4 kg 

link 6 center of gravity location r cgt g = [0, 0, 0] T (m) 
link 6 inertia tensor /«, s = diag[0. 1,0. 1,0.1] (kgm 2 ) 
link 7 mass m 7 = 4 kg 

link 7 center of gravity location r cgf 7 — [ 0 , 0 , 0 ] T (m) 
link 7 inertia tensor A,* = diag[ 0 . 1 , 0 . 1 , 0 . 1 ] (kgm 2 ) 
end effector mass m« = 1 kg 

end effector center of gravity location r_ cg — [0,0, 0] T (m) 
end effector inertia tensor = diag[0. 1,0. 1,0.1] (kgm 2 ) 
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